#include "test.h"
#include "Global.h"
#include "Dog.h"
#include "SBUS.h"

void Key_Callback(void)
{
}

void MM_TIM_Callback(void)
{
    static uint8_t count;

    if (count++ > 30)
    {
        count = 0;
        // UART_Print("%.3f %.3f\n", dog.motors[4].Position, dog.motors[5].Position);
        // UART_Print("%.3f\n", global.phase[0]);
        // UART_Print("alpha:%.3f, beta:%.3f\n", dog.legs[0].alpha, dog.legs[0].beta);
        // UART_Print("%d %f\n", global.isWalking, global.throttle);
        // UART_Print("%f %f\n", DEG(dog.legs[0].alpha), DEG(dog.legs[0].beta));
        UART_Print("%f %f %f %f\n", dog.legs[0].S, dog.legs[1].S, dog.legs[2].S, dog.legs[3].S);

        // UART_Print("%d\n", sbus_channels[1]);
    }
}

void Test_Init(void)
{
    AttachInterrupt_GPIO(KEY_Pin, Key_Callback);
    AttachInterrupt_TIM(&htim3, MM_TIM_Callback);
    HAL_TIM_Base_Start_IT(&htim3);
    UART_Print("Enter test mode\n");
}

void Test_Run(void)
{
}
